/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package spectrum;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
/**
 *
 * @author JAG
 */
public class HW {
    public static CANJaguar Jag1, Jag2, Jag3, Jag4;
    
    
    public static RobotDrive SpectrumDrive;
    
    //THIS IS A GIT TEST
    public static DoubleSolenoid sol_1 = new DoubleSolenoid(1,2);
    
    
    
    
    public static PIDController pid_object = new PIDController(0.0341,0,0,HW.left_encoder,HW.Jag1);
    public static double PID_DISTANCE = 0.0;
    public static Encoder left_encoder = new Encoder(3,4);
    
    
    public static Compressor compressor = new Compressor(5,3);
    
    public static Joystick driveStickLeft = new Joystick(1);
    public static Button pid_button = new JoystickButton(driveStickLeft,3);
    public static Joystick driveStickRight = new Joystick(2);

    public static CANJaguar getJaguar(int id) {
        CANJaguar jag = null;
        int i = 0;
        while (jag == null && 1 < 10) {
            try {
                System.out.println("Trying to connect to CAN port " + id + "...");
                jag = new CANJaguar(id);
            } catch (Exception e) {
                System.out.println(e.getMessage());
            }
            i++;
        }
        System.out.println("Connected to CAN port " + id);
        return jag;
    }
    
    public static void pidEnable(double inches)
    {
       pid_object.enable();
       pid_object.setSetpoint(PID_DISTANCE);
       pid_object.setOutputRange(-0.5, 0.5);
        try{
        
        //utilities.Utils.debugPrintln("PIDget: " + pid_object.get());
     
        HW.Jag3.setX(HW.Jag1.getX());
        HW.Jag2.setX(-1*HW.Jag1.getX());
        HW.Jag4.setX(HW.Jag2.getX());
            utilities.Utils.debugPrintln(HW.left_encoder.getDistance() + " inches");
            utilities.Utils.debugPrintln("PID VALUE: " + pid_object.get());
            utilities.Utils.debugPrintln("PID IS ENABLED: " + pid_object.isEnable());
            utilities.Utils.debugPrintln("SETPOINT: " + pid_object.getSetpoint());
            utilities.Utils.debugPrintln("ERROR DISTANCE: " + pid_object.getError());
       
        }catch (CANTimeoutException e1) {
          // TODO Auto-generated catch block
          e1.printStackTrace();
      }
        
    }
    
    public static void encoderStart()
    {
        HW.left_encoder.setDistancePerPulse(0.03222146311374146911243736803364);
         HW.left_encoder.start();
    }
    
    
}
